Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- #include <CheapStepper.h>
- //define I/O ports
- #define inputSelector A5
- #define limitSwitch A4
- #define spindleRotation 2
- #define LED1 3
- #define LED2 4
- #define startSwitch 5
- #define stopSwitch 6
- #define spindleServo 7
- #define UN2003IN4 9
- #define UN2003IN3 10
- #define UN2003IN2 11
- #define UN2003IN1 12
- #define outputVoltage A1
- #define potentioMeter A2
- //define stepper speed
- #define stepperNormalSpeed 2
- #define stepperFastSpeed 20
- #define dangerLowVoltage 230
- #define hysteresis 5
- #define stepperMaxTravel 245760
- #define holdOffDistance 3000
- //define variables
- int currentPotPosition = 0;
- int currentVoltage = 0;
- int lowerHysteresis = 0;
- int higherHysteresis = 0;
- int startLocation = 0;
- int shortCircuitLocation = 0;
- bool startLocationSet = false;
- bool moveBackward = true;
- bool moveUp = false;
- bool moveDown = false;
- bool starT = false;
- bool stoP = false;
- int stepsLeft = 0;
- static unsigned int stateMachine = 0; // Initialize the state as "inactive"
- //Setup servos
- Servo spindle;
- CheapStepper stepper (12,11,10,9);
- // Simple macros to declare debounce input pins.. Saves time typing.
- #define DEBOUNCE_DECLARE(p) \
- bool p ## _state = 0; \
- int p ## _count = 0;
- // macro to debounce a pin.
- #define debounceCount 10
- #define DEBOUNCE(p) do { \
- if(digitalRead(p) == HIGH) { \
- p ## _count++; \
- if(p ## _count >= debounceCount) { \
- p ## _state = 1; \
- } else { \
- p ## _state = 0; \
- } \
- } else { \
- p ## _count = 0; \
- p ## _state = 0; \
- } \
- } while(0) /* We use do{ }while(0) because this allows us to put a ";" behind the macro when using it. */
- // Declare the variables we use to store the debounced state in.
- DEBOUNCE_DECLARE(inputSelector);
- DEBOUNCE_DECLARE(spindleRotation);
- DEBOUNCE_DECLARE(startSwitch);
- DEBOUNCE_DECLARE(stopSwitch);
- DEBOUNCE_DECLARE(limitSwitch);
- void setup(){
- // Configure I/O
- pinMode(inputSelector, INPUT);
- pinMode(limitSwitch, INPUT);
- pinMode(spindleRotation, INPUT);
- pinMode(LED1, OUTPUT);
- pinMode(LED2, OUTPUT);
- pinMode(startSwitch, INPUT);
- pinMode(stopSwitch, INPUT);
- pinMode(spindleServo, OUTPUT);
- pinMode(UN2003IN1, OUTPUT);
- pinMode(UN2003IN2, OUTPUT);
- pinMode(UN2003IN3, OUTPUT);
- pinMode(UN2003IN4, OUTPUT);
- pinMode(outputVoltage, INPUT);
- pinMode(potentioMeter, INPUT);
- spindle.attach(spindleServo); // Attach pulse output pin for spindle servo
- // Serial.begin(115200);
- }
- void loop(){
- // Debounce our input pins.
- DEBOUNCE(stopSwitch);
- DEBOUNCE(startSwitch);
- DEBOUNCE(spindleRotation);
- DEBOUNCE(inputSelector);
- DEBOUNCE(limitSwitch);
- if(inputSelector_state == 1){
- if (startSwitch_state == 1 && stopSwitch_state == 0){
- moveUp = true;
- moveDown = false;
- } else if (stopSwitch_state == 1 && startSwitch_state == 0){
- moveUp = false;
- moveDown = true;
- } else {
- moveUp = false;
- moveDown = false;
- }
- } else
- {
- moveUp = false;
- moveDown = false;
- if(startSwitch_state == 1 && stopSwitch_state == 0){
- starT = true;
- stoP = false;
- } else if (stopSwitch_state == 1 && startSwitch_state == 0){
- starT = false;
- stoP = true;
- }else {
- starT = false;
- stoP = false;
- }
- }
- stepsLeft = stepper.getStepsLeft();
- switch (stateMachine){
- case 0: // Standby state.
- //Serial.println("Case 0, Standby");
- if(moveUp == 1){
- /*stepper.setRpm(stepperFastSpeed);
- moveBackward = true;
- stepper.newMove (moveBackward, 20);
- stepper.run(); */
- stepper.step(1);
- } else if(moveDown == 1){
- /*stepper.setRpm(stepperFastSpeed);
- moveBackward = false;
- stepper.newMove (moveBackward, 20);
- stepper.run();*/
- stepper.step(0);
- } else{
- stepper.stop();
- }
- if(starT == 1){ // Start switch activated
- digitalWrite(LED1, 1); // Turn on LED
- stepper.setRpm(stepperNormalSpeed); //Set stepper normal speed for regular runtime
- moveBackward = false; // Run forward
- stepper.newMoveTo (moveBackward, stepperMaxTravel); //number of degrees to move forward. (This is full the travel of the ballscrew)
- stateMachine = 1;
- }
- break;
- case 1: // Run state. The LED is on, wait for instructions to stop.
- //Serial.println("Case 1, RUN");
- //if(spindleRotation_state == 1) {
- //Serial.println("SpindleRotation_state = 1");
- // spindle.write(180); // Set spindle rotation at full speed (180=full speed clockwise, 90=stop, 0=full speed counter clockwise)
- //} else{
- //Serial.println("SpindleRotation_state = 0");
- // spindle.write(90); //stop spindle rotation
- //}
- currentVoltage = analogRead(outputVoltage); //reads the voltage number from the analog input
- currentPotPosition = analogRead(potentioMeter); //Reads the position of the potentiometer
- lowerHysteresis = (currentPotPosition - hysteresis); //Sets lower hysteresis
- higherHysteresis = (currentPotPosition + hysteresis); //Sets higher hysteresis
- if(stoP == 1 or limitSwitch_state == 1){ // When this input is high, go to stop
- digitalWrite(LED1, 0); // Switch off LED.
- stepper.stop(); //stop stepper
- spindle.write(90); //stop spindle rotation
- stateMachine = 2;
- }
- if(higherHysteresis > currentVoltage > lowerHysteresis) { // hold it right there, thats the spot!
- stepper.stop();
- if(startLocationSet == true){
- startLocationSet = true;
- startLocation = stepper.getStep(); //Get the location of the first hysteresis (close to surface)
- }
- } else if(currentVoltage <= dangerLowVoltage) { //Detecting short circuit
- stateMachine = 3;
- } else {
- stepper.newMoveTo (moveBackward, stepperMaxTravel); //number of degrees to move forward. (This is full the travel of the ballscrew)
- stepper.run();
- }
- break;
- case 2: // Stop state
- //Serial.println("Case 2, STOP");
- moveBackward = true; //Reverse stepper direction
- stepper.setRpm(stepperFastSpeed); // Set fast step speed for retraction.
- stepper.newMoveTo(moveBackward, 0); //Set stepper location to start position
- if(stepsLeft == 0){
- stateMachine = 4;
- } else {
- stepper.run();
- }
- break;
- case 3: // short circut
- //Serial.println("Case 3, Short circuit");
- stepper.stop(); //stop stepper
- shortCircuitLocation = stepper.getStep(); //Get the location of the short circuit
- moveBackward = true; //change direction of movement
- stepper.setRpm(stepperFastSpeed); //sets fast speed for stepper
- stepper.newMoveTo(moveBackward, startLocation); //makes stepper move to start position
- stateMachine = 5;
- break;
- case 4: // Reset variables & return to standby
- //Serial.println("Case 4, Reset");
- //reset all variables
- stateMachine = 0;
- break;
- case 5: // short circuit backwards movement
- //Serial.println("Case 5, Short circuit back");
- if(stepsLeft == 0){ //if movement is done
- stepper.stop(); //stop stepper
- moveBackward = false; //change direction of movement
- stepper.newMoveTo(moveBackward, shortCircuitLocation - holdOffDistance); //Move back close to the point of short circuit
- stateMachine = 6; //set stateMachine to short circuit forward movement
- } else {
- stepper.run(); //continue running stepper
- }
- break;
- case 6: // short circuit forwards movement
- //Serial.println("Case 6, Short circuit forward");
- if(stepsLeft == 0){ //if movement is done
- moveBackward = false;
- stepper.newMoveTo(moveBackward, stepperMaxTravel);
- stateMachine = 1; //set stateMachine to running
- stepper.setRpm(stepperNormalSpeed);
- } else{
- stepper.run(); //Continue running stepper
- }
- break;
- default: // Unknown state!
- //Serial.println("Case default, unknown state");
- stateMachine = 0; // Go to standby.
- break;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement